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Abstract: 

Our  research  had  three  main  thrusts:  (i)  Optimization-based  motion  planning: 
Traditional  approaches  to  motion  planning  tend  to  slow  down  in  high-dimensional 
spaces  and  for  curvature  constrained  problems.  We  developed  an  optimization- 
based  approach  that  can  efficiently  solve  such  problems,  (ii)  Belief  space  planning: 
To  account  for  uncertainty  about  the  environment  and  robot  belief  space  planning 
tries  to  find  a  plan  that  optimizes  (on  expectation)  the  sequence  of  probability 
distributions  that  would  result  from  that  plan.  As  a  consequence  belief  space 
planning  anticipates  (and  accounts  for)  sensory  information,  the  expected 
magnitude  of  perturbations,  and  controllability.  We  made  contributions  in  handling 
collisions,  handling  occlusions,  scaling  up  belief  space  planning,  going  beyond 
unimodal  beliefs,  and  hierarchical  belief  space  planning,  (iii)  Learning  from 
demonstrations:  We  developed  an  approach  to  generalize  demonstrated  motion  in 
training  scenarios  to  test  scenarios.  At  the  core  of  our  approach  is  non-rigid 
registration  to  warp  the  training  scene  onto  the  test  scene.  While  registration  is  only 
concerned  with  the  objects  and  their  environment,  we  show  that  it  is  possible  to 
meaningfully  extrapolate  the  registration  and  to  also  warp  the  robot  gripper 
trajectories  from  training  scene  to  test  scene.  Our  approach  is  particularly  appealing 
for  the  manipulation  of  deformable  objects,  which  present  the  robot  with  a  very 
high-dimensional  state  space  and  large  amounts  of  variability,  making  them 
particularly  challenging  for  robots  to  manipulate.  It  has  enabled  autonomous  knot 
tying  for  a  wide  range  of  knot-types  and  automation  of  some  simplified  suturing 
tasks. 
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Annual  Accomplishments: 

Our  research  had  three  main  thrusts:  (i)  Optimization-based  motion  planning,  (ii) 
Belief  space  planning,  (iii)  Learning  from  demonstrations. 

(i)  Optimization-based  motion  planning 

Traditional  approaches  to  motion  planning  tend  to  slow  down  in  high-dimensional 
spaces  and  for  curvature  constrained  problems  (such  as  needle  steering  and  ribbon 
routing).  We  developed  an  optimization-based  approach  that  can  efficiently  solve 
such  problems  [58,  68,  74].  In  this  work  we  introduced  a  new  optimization-based 
approach  for  robotic  motion  planning  among  obstacles.  Like  CHOMP  (Covariant 
Hamiltonian  Optimization  for  Motion  Planning),  our  algorithm  can  be  used  to  find 
collision-free  trajectories  from  naive,  straight-line  initializations  that  might  be  in 
collision.  At  the  core  of  our  approach  are  (a)  a  sequential  convex  optimization 
procedure,  which  penalizes  collisions  with  a  hinge  loss  and  increases  the  penalty 
coefficients  in  an  outer  loop  as  necessary,  and  (b)  an  efficient  formulation  of  the  no¬ 
collisions  constraint  that  directly  considers  continuous-time  safety.  Our  algorithm 
is  implemented  in  a  software  package  called  TrajOpt.  We  validated  our  approach 
through  a  series  of  experiments  comparing  TrajOpt  with  CHOMP  and  randomized 
planners  from  OMPL,  with  regard  to  planning  time  and  path  quality.  We  considered 
motion  planning  for  7  DOF  robot  arms,  18  DOF  full-body  robots,  statically  stable 
walking  motion  for  the  34  DOF  Atlas  humanoid  robot,  and  physical  experiments 
with  the  18  DOF  PR2.  We  also  applied  TrajOpt  to  plan  curvature-constrained 
steerable  needle  trajectories  in  the  SE(  3)  configuration  space  and  multiple  non¬ 
intersecting  curved  channels  within  3D-printed  implants  for  intracavitary 
brachytherapy. 

(ii)  Belief  space  planning 

Uncertainty  about  the  environment  is  unavoidable  in  unstructured  settings. 
Uncertainty  about  the  robot  itself  can  also  be  a  factor,  especially  as  robots  become 
cheaper  and  cheaper.  Belief  space  planning  algorithms  try  to  find  a  plan  that 
optimizes  (on  expectation)  the  sequence  of  probability  distributions  that  would 
result  from  that  plan.  As  a  consequence  belief  space  planning  anticipates  (and 
accounts  for)  sensory  information,  the  expected  magnitude  of  perturbations,  and 
controllability. 

Building  on  advances  in  sequential  convex  optimization  our  most  recent  work  is 
able  to  find  (locally)  optimal  paths  in  Gaussian  belief  spaces.  Our  key  contributions 
have  been  in  handling  probabilistic  collision  checking  for  articulated  robots  [62],  in 
handling  sensors  with  a  limited  field  of  view  and  occlusions  [69],  and  in  exploiting 
the  structure  of  Gaussian  belief  space  planning  problems  to  enable  scaling  up  to 
significantly  higher  dimensional  problems  (i.e.,  higher  number  of  degrees  of 
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freedom  of  the  robot,  higher  number  of  objects,  and  higher  number  of  landmarks) 
than  was  possible  before  [75]. 

Gaussian  belief  space  planning,  while  surprisingly  effective,  nevertheless  has 
significant  limitations  as  it  is  only  able  to  reason  about  unimodal  beliefs.  In  [C89] 
we  have  started  to  investigate  multimodal  belief  representations  in  the  context  of 
grasping  occluded  objects. 

Recent  work  has  made  headway  on  integration  of  task  and  motion  planning  in  state 
space.  We  developed  Interfaced  Belief  Space  Planning  (IBSP):  a  modular  approach 
to  task  and  motion  planning  in  belief  space.  We  use  a  task-independent  interface 
layer  to  combine  an  off-the-shelf  classical  planner  with  motion  planning  and 
inference.  We  determinize  the  problem  under  the  maximum  likelihood  observation 
assumption  to  obtain  a  deterministic  representation  where  successful  plans 
generate  goal-directed  observations.  We  leverage  properties  of  maximum  likelihood 
observation  determinizations  to  obtain  a  simple  representation  of  (optimistic)  belief 
space  dynamics  that  is  well  suited  to  planning.  Our  interface  is  implemented  with 
standard  belief  state  queries,  requiring  only  the  ability  to  sample,  compute 
unnormalized  likelihoods,  and  compute  maximum  likelihood  states.  Our 
contribution  is  a  novel  algorithm  for  task  and  motion  planning  in  belief  space  that 
has  minimal  dependence  on  the  details  of  the  inference  engine  used.  IBSP  can  work 
with  a  broad  class  of  black  box  state  estimators,  with  zero  changes  to  the  algorithm. 
We  validate  our  approach  in  simulated  tasks  for  the  PR2  that  account  for  continuous 
state,  different  types  of  initial  state  distributions,  and  negative  observations. 

(iii)  Learning  from  demonstrations 

In  [C64]  we  have  started  to  develop  the  beginnings  of  a  theory  that  allows  for 
generalization  from  training  scenarios  to  test  scenarios.  At  the  core  of  our  approach 
is  non-rigid  registration  to  warp  the  training  scene  (where  the  demonstration 
happens)  onto  the  test  scene  (which  is  a  previously  unseen  situation  where  the 
robot  has  to  autonomously  succeed).  While  registration  is  only  concerned  with  the 
objects  and  their  environment,  we  show  that  it  is  possible  to  meaningfully 
extrapolate  the  registration  and  to  also  warp  the  robot  gripper  trajectories  from 
training  scene  to  test  scene.  Our  approach  is  particularly  appealing  for  the 
manipulation  of  deformable  objects,  which  present  the  robot  with  a  very  high¬ 
dimensional  state  space  and  large  amounts  of  variability,  making  them  particularly 
challenging  for  robots  to  manipulate.  Our  approach  has  enabled  autonomous  knot 
tying  for  a  wide  range  of  knot-types  and  starting  configurations.  It  has  enabled 
automation  of  some  simplified  suturing  tasks.  Figure  1  illustrates  some  of  our 
experiments. 
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Figure  1:  Autonomous  knot-tying,  suturing,  and  folding  of  a  t-shirt.  Key  insight  in  this 
project  was  how  to  build  on  existing  techniques  for  non-rigid  registration  to  enable 
generalization  of  motions  by  warping  them  appropriately  from  training  scene  to  test 
scene. 

Our  approach  as  discussed  thus  far  only  considers  robot  gripper  pose  trajectories, 
but  often  what  matters  forces  and  torques  the  robot  applies.  For  instance,  tightening 
a  knot  requires  pulling  the  ends,  flattening  an  article  of  clothing  requires  smoothing 
out  wrinkles,  and  erasing  a  whiteboard  requires  applying  downward  pressure.  In 
[C92]  we  show  how  the  variation  between  multiple  demonstrations  of  variants  of 
the  same  task  can  be  used  to  determine  when,  and  to  which  extent,  forces  than  well 
pose  matter.  This  results  in  a  learned  variable  impedance  control  strategy  that 
trades  off  force  and  position  errors,  providing  the  right  level  of  compliance  for  each 
motion. 

Improved  registration  [C87] :  The  first  step  in  our  approach  is  a  non-rigid 
registration  between  training  scene  and  test  scene.  The  current  registration  only 
considers  a  geometric  representation  of  train  and  test  scenes  and  it  considers  all 
parts  of  the  scenes  equally  important.  In  [C87]  we  show  how  to  incorporate  surface 
normals.  Future  work  will  study  how  to  incorporate  visual  (in  addition  to 
geometric)  features. 
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